#include "WPILib.h"
#include "Constants.h"
#include "Turret.h"
#include "Basket.h"
#include "Shooter.h"
#include "TargetingCamera.h"
#include "ShooterEncoder.h"
#include "SmartJoypad.h"
//#include "NetworkTables/NetworkTable.h"
#include <string> 

#include <cstdio>
#include <cstdlib>


class Team830Robot : public IterativeRobot
{
	static const bool USE_SQUARED_INPUTS = false;
	
	// Driver Station Input
	static const int JOYSTICK_1 = 1;
	static const int JOYSTICK_2 = 2;
	
	// PWM Channels
	static const int VICTOR_LEFT_DRIVE = 2;
	static const int VICTOR_RIGHT_DRIVE = 9;
	static const int VICTOR_TURRET_FLYWHEEL_1 = 6;
	static const int VICTOR_TURRET_FLYWHEEL_2 = 5;
	static const int VICTOR_BALL_CONVEYOR = 4;
	static const int VICTOR_BRIDGE_ARM = 7;
	static const int VICTOR_BALLFEED = 1;
	
	// Digital I/O Channels
	static const int DIO_COMPRESSOR_SWITCH = 1;
	static const int DIO_IR_BREAK = 14;
	static const int DIO_BRIDGE_ARM_LIMIT_SWITCH_HIGH = 3;
	static const int DIO_BRIDGE_ARM_LIMIT_SWITCH_LOW = 5;
	static const int VICTOR_BRAKE_COAST = 2;
	
	// Relay Channels
	
	static const int RELAY_COMPRESSOR = 1; 
	
	// Analog Channels
<<<<<<< .mine

=======
>>>>>>> .r122
	
	//Solenoid channels
	static const int SOLENOID_FLIPPY_BAR = 5;

	static const int SOLENOID_GEAR_SHIFT_FORWARD = 1;
	static const int SOLENOID_GEAR_SHIFT_REVERSE = 3;

	
	//Other declarations
	static const bool FLIPPY_BAR_OPEN = false;
	static const bool FLIPPY_BAR_CLOSE = true;
	static const bool VICTOR_COAST = true;
	static const bool VICTOR_BRAKE = false;
	
	static const float DISTANCE_OFFSET = 1.2;
	
	static const int AUTON_TIME = 6;

	// Input Members
	
	SmartJoypad* pilot;
	SmartJoypad* copilot;
	
	SmartDashboard* dashboard;
	NetworkTable* team830Table;
	
	// Output Members
	
	RobotDrive* drive;
	Shooter* shooter;
	Turret* turret;
	TargetingCamera* camera;
	DoubleSolenoid* gearShifter;
	Compressor* compressor;
	DigitalOutput* victorMode;
	
	Victor* bridgeArm;
	DigitalInput* bridgeArmSwitchHigh;
	DigitalInput* bridgeArmSwitchLow;
	
	
	// Aiming variables
	float distance;
	float shooterSpeed;

	bool tankDrive;
	
	Timer* autofireTimer;
	Timer* autonTimer;
	
	bool brakeOrCoast;
	
	int autonStage;
	int firingState;

	int leftright, updown;
	
public:
	
	Team830Robot(void)
	{
		pilot = new SmartJoypad(JOYSTICK_1);
		copilot = new SmartJoypad(JOYSTICK_2);
		
		tankDrive = false;
		
		// The victors are created explicitly and then passed to the RobotDrive object, 
		// because the RobotDrive constructor expects Jaguars by default.
		drive = new RobotDrive(
			new Victor(VICTOR_LEFT_DRIVE),
			new Victor(VICTOR_RIGHT_DRIVE)
		);
		
		shooter = new Shooter(
			VICTOR_TURRET_FLYWHEEL_1, 
			VICTOR_TURRET_FLYWHEEL_2,
			DIO_IR_BREAK
		);
		
		turret = new Turret(
			VICTOR_BALLFEED,
			VICTOR_BALL_CONVEYOR,
			SOLENOID_FLIPPY_BAR
		);
		
		camera = new TargetingCamera();
		
		gearShifter = new DoubleSolenoid(SOLENOID_GEAR_SHIFT_FORWARD, SOLENOID_GEAR_SHIFT_REVERSE);
		
		compressor = new Compressor(DIO_COMPRESSOR_SWITCH, RELAY_COMPRESSOR);
		compressor->Start();
		
		bridgeArm = new Victor(VICTOR_BRIDGE_ARM);
		bridgeArmSwitchHigh = new DigitalInput(DIO_BRIDGE_ARM_LIMIT_SWITCH_HIGH);
		bridgeArmSwitchLow = new DigitalInput(DIO_BRIDGE_ARM_LIMIT_SWITCH_LOW);
		
		victorMode = new DigitalOutput(VICTOR_BRAKE_COAST);
		victorMode->Set(VICTOR_COAST);
		brakeOrCoast = VICTOR_COAST;
		
		autofireTimer = new Timer();
		autofireTimer->Start();
		
		autonTimer = new Timer();
		autonTimer->Start();
		
		firingState = 0;
		
		dashboard 		= SmartDashboard::GetInstance();
		team830Table 	= NetworkTable::GetTable("Team830");
		
		leftright = 0;
		updown = 0;
		
		dashboard->PutString("RPS:", "2");
	}
	
	~Team830Robot()
	{
		delete pilot;
		delete copilot;
		
		delete drive;
		delete shooter; 
		delete turret;
		delete camera;
		
		delete gearShifter;
		delete autofireTimer;
	}
	
	/********************************** Little Subroutines *********************************/
	
	void AutoFire(int &firingState, int shooterSpeed)
		{
			/*
			1) Flippy bar back
			2) Flywheel back, ~.1 seconds (to clear clogs)
			3) Flippy bar in
			4) PID to wheel speed
			5) Conveyor up (slowish speed)
			6) Flywheel off
			*/
			switch (firingState)
			{
				case 0: // Not firing.
					break;
					
				case 1:
					autofireTimer->Reset();
					turret->SetFlippyBar(FLIPPY_BAR_OPEN);
					shooter->EnablePIDs(false);
					turret->SetBallConveyor(-1.0f);
					turret->SetBallfeed(-1.0f); // Set the ballfeed to reverse.
					firingState++;
					break;
				
<<<<<<< .mine
				case 2: // Wait for the System to clear any balls, then prepare to load the new ball.
					if (autofireTimer->Get() > 0.25)
					{
						turret->SetFlippyBar(FLIPPY_BAR_CLOSE);
						shooter->SetSpeed(shooterSpeed + updown);
						shooter->EnablePIDs(true); 
						turret->SetBallfeed(0.0f);
						turret->SetBallConveyor(0.0f);
						firingState++;
					}
					break;
					
				case 3: // Run the flywheel at the correct speed.
=======
			case 1:
				autofireTimer->Reset();
				turret->SetFlippyBar(FLIPPY_BAR_OPEN);
				shooter->EnablePIDs(false);
				turret->SetBallConveyor(-1.0f);
				turret->SetBallfeed(-1.0f); // Set the ballfeed to reverse.
				firingState++;
				break;
			
			case 2: // Wait for the System to clear any balls, then prepare to load the new ball.
				if (autofireTimer->Get() > 0.15)
				{
>>>>>>> .r122
					if (shooter->UpToSpeed())
						firingState++;
					break;
					
				case 4: // Move the ball into the shooter to shoot the ball.
					turret->SetFlippyBar(FLIPPY_BAR_CLOSE);
					if (shooter->UpToSpeed())
					{
						turret->SetBallConveyor(1.0f);
						turret->SetBallfeed(1.0f); // Set ballfeed to forward.
					}
					else
					{
						turret->SetBallConveyor(0.0f);
						turret->SetBallfeed(0.0f); // Set ballfeed to forward.
					}
					break;
					
				case 5: // Turn everything off.
					turret->SetBallConveyor(0.0);
					turret->SetBallfeed(0.0f); // Turn ballfeed off.
					turret->SetFlippyBar(FLIPPY_BAR_OPEN);
					shooter->EnablePIDs(false);
					shooter->SetSpeed(0.0);
					shooter->SetRawSpeed(0.0);
					firingState = 0;
					break;
					
				default:
					firingState = 0;
			}
		}
	void spinFlywheel(int &firingState, int shooterSpeed)
		{
			switch (firingState)
			{
				case 0: // Not firing.
					break;
					
				case 1:
					autofireTimer->Reset();
					turret->SetFlippyBar(FLIPPY_BAR_OPEN);
					shooter->EnablePIDs(false);
					turret->SetBallConveyor(-1.0f);
					turret->SetBallfeed(-1.0f); // Set the ballfeed to reverse.
					firingState++;
					break;
				
<<<<<<< .mine
				case 2: // Wait for the System to clear any balls, then prepare to load the new ball.
					if (autofireTimer->Get() > 0.25)
					{
						turret->SetFlippyBar(FLIPPY_BAR_CLOSE);
						shooter->SetSpeed(shooterSpeed + updown);
						shooter->EnablePIDs(true); 
						turret->SetBallfeed(0.0f);
						turret->SetBallConveyor(0.0f);
						firingState++;
					}
					break;
					
				case 3: // Run the flywheel at the correct speed.
					if (shooter->UpToSpeed())
						firingState++;
					break;
					
				case 4: // Chillax
					
					break;
					
				case 5: // Turn everything off.
					turret->SetBallConveyor(0.0);
					turret->SetBallfeed(0.0f); // Turn ballfeed off.
					turret->SetFlippyBar(FLIPPY_BAR_OPEN);
					shooter->EnablePIDs(false);
					shooter->SetSpeed(0.0);
					shooter->SetRawSpeed(0.0);
					firingState = 0;
					break;
					
				default:
					firingState = 0;
			}
=======
			case 3: // Run the flywheel at the correct speed.
				if (shooter->UpToSpeed())
					firingState++;
				break;
				
			case 4: // Move the ball into the shooter to shoot the ball.
				if (shooter->UpToSpeed())
				{
					//turret->SetBallConveyor(1.0f);
					turret->SetBallConveyor(0.6f);
					turret->SetBallfeed(1.0f); // Set ballfeed to forward.
				}
				else
				{
					turret->SetBallConveyor(0.0f);
					turret->SetBallfeed(0.0f); // Set ballfeed to forward.
				}
				break;
				
			case 5: // Turn everything off.
				turret->SetBallConveyor(0.0);
				turret->SetBallfeed(0.0f); // Turn ballfeed off.
				shooter->EnablePIDs(false);
				shooter->SetSpeed(0.0);
				shooter->SetRawSpeed(0.0);
				firingState = 0;
				break;
				
			default:
				firingState = 0;
>>>>>>> .r122
		}
	
	void MoveConveyor() {
		if (shooter->UpToSpeed()){
			turret->SetBallConveyor(1.0f);
			turret->SetBallfeed(1.0f);
		}
		else
		{
			turret->SetBallConveyor(0.0f);
			turret->SetBallfeed(0.0f);
		}
		turret->SetFlippyBar(FLIPPY_BAR_CLOSE);
	}
	
	void SpinFlywheel() {
		switch (firingState)
		{
			case 0: // Not firing.
				break;
				
			case 1:
				autofireTimer->Reset();
				turret->SetFlippyBar(FLIPPY_BAR_OPEN);
				shooter->EnablePIDs(false);
				turret->SetBallConveyor(-1.0f);
				turret->SetBallfeed(-1.0f); // Set the ballfeed to reverse.
				firingState++;
				break;
			
			case 2: // Wait for the System to clear any balls, then prepare to load the new ball.
				if (autofireTimer->Get() > 0.25)
				{
					turret->SetFlippyBar(FLIPPY_BAR_CLOSE);
					shooter->SetSpeed(shooterSpeed + updown);
					shooter->EnablePIDs(true); 
					turret->SetBallfeed(0.0f);
					turret->SetBallConveyor(0.0f);
					firingState++;
				}
				break;
				
			case 3: // Run the flywheel at the correct speed.
				/*if (shooter->UpToSpeed())
					firingState++;*/
				break;
				
			case 4: // Turn everything off.
				turret->SetFlippyBar(FLIPPY_BAR_OPEN);
				shooter->EnablePIDs(false);
				shooter->SetSpeed(0.0);
				shooter->SetRawSpeed(0.0);
				firingState = 0;
				break;
				
			default:
				firingState = 0;
		}

		
	}
	
	/********************************** Init Routines *************************************/
	
	void RobotInit(void)
	{
		drive->SetExpiration(0.1);
	}
	
	void DisabledInit(void)
	{
		drive->TankDrive(0.0, 0.0);
	}
	
	void AutonomousInit(void)
	{
		drive->TankDrive(0.0, 0.0);
		
		autonTimer->Reset();
		
		autonStage = 0;
		firingState = 1;
	}

	void TeleopInit(void)
	{
		drive->TankDrive(0.0, 0.0);
		
		firingState = 0;
	}

	/********************************** Periodic Routines *************************************/
	
	void DisabledPeriodic(void) 
	{
		if (pilot->GetAxis(SmartJoypad::DPAD_Y) == 1.0)
			tankDrive = false;
		else if (pilot->GetAxis(SmartJoypad::DPAD_Y) == -1.0)
			tankDrive = true;
		
		shooter->EnablePIDs(false);
		//pidBalance->Disable();
		shooter->Update();
	}
	
	void AutonTipBridge(void){
		shooter->Update();
		switch (autonStage)
		{
			//put down the bridge-tipper
			case 0: 
				drive->TankDrive(0.0, 0.0);
				//switch returns true if not pressed, false if pressed
				if (bridgeArmSwitchLow->Get())
					bridgeArm->Set(0.2f);
				else {
					bridgeArm->Set(0.0f);
					autonTimer->Reset();
					autonTimer->Start();
					autonStage++;
				}
				break;
			//drive Forward to push down the bridge
			case 1: 
				drive->TankDrive(-1.0, -1.0);
				if (autonTimer->Get() >= AUTON_TIME){
					drive->TankDrive(0.0, 0.0);
					autonStage++;
				}
				break;
			default:
				drive->TankDrive(0.0, 0.0);
				break;
		}
	}
	
	void AutonFire(void) {
		shooter->Update();
		drive->TankDrive(0.0, 0.0);
		switch(autonStage){
			case 0: // Find the target.
				if(camera->FindTarget(distance))
				autonStage++; 
				break;
				
			case 1: // Adjust the turret, and wait for it to be on target.
				/*if (turret->OnTarget())
				{
					autonTimer->Reset();
					autonStage++;
				}*/
				autonTimer->Reset();
				autonStage++;
				
				break;
			
			case 2:
				//if (autonTimer->Get() > 2.0)
				autonStage++;
				break;
				
			case 3:
				shooterSpeed = shooter->ShootTo(distance + DISTANCE_OFFSET);
				AutoFire(firingState, (int) shooterSpeed);
				break;
			default:
				break;
		}
	}
	
	void AutonTipAndFire(void) {
		shooter->Update();
		switch (autonStage)
		{
		case 0: // Find the target.
			drive->TankDrive(0.0, 0.0);
			if(camera->FindTarget(distance))
				autonStage++;
			break;
			
		case 1: // Adjust the turret, and wait for it to be on target.
			drive->TankDrive(0.0, 0.0);
			
			autonTimer->Reset();
			autonStage++;
				
			break;
		
		case 2:
			drive->TankDrive(0.0, 0.0);
			//if (autonTimer->Get() > 2.0)
			autonStage++;
			autonTimer->Reset();
			break;
			
		case 3:
			drive->TankDrive(0.0, 0.0);
			shooterSpeed = shooter->ShootTo(distance + DISTANCE_OFFSET);
			AutoFire(firingState, (int) shooterSpeed);
			if (autonTimer->Get() > 8.0)
				autonStage++;
			break;
			
		case 4:
			drive->TankDrive(0.0, 0.0);
			//switch returns true if not pressed, false if pressed
			if (bridgeArmSwitchLow->Get())
				bridgeArm->Set(0.2f);
			else {
				bridgeArm->Set(0.0f);
				autonTimer->Reset();
				autonStage++;
			}
			break;
			
		case 5: 
			drive->TankDrive(0.0, 0.0);
			if (autonTimer->Get() >= AUTON_TIME){
				drive->TankDrive(0.0, 0.0);
				autonStage++;
			}
			break;
			
		default:
			drive->TankDrive(0.0, 0.0);
			break;
		}
	}
	
	void AutonomousPeriodic(void)
	{
		//High Gear
		gearShifter->Set(DoubleSolenoid::kReverse);
		
		//decided by button on dashboard
		switch (team830Table->GetInt("AutonMode")){
		case 0:
			this->AutonFire();
			break;
		case 1:
			this->AutonTipBridge();
			break;
		case 2:
			this->AutonTipAndFire();
			break;
		case 3:
			drive->TankDrive(0.0, 0.0);
			break;
		default:
			this->AutonTipAndFire();
			break;
		}
	}
	
	void TeleopPeriodic(void)
	{	
		if (pilot->GetButtonPress(SmartJoypad::FACE_W) && brakeOrCoast == VICTOR_COAST){
			brakeOrCoast = VICTOR_BRAKE;
			victorMode->Set(VICTOR_BRAKE);
		} else if (pilot->GetButtonPress(SmartJoypad::FACE_E) && brakeOrCoast == VICTOR_BRAKE){
			brakeOrCoast = VICTOR_COAST;
			victorMode->Set(VICTOR_COAST);
		}
		
		// Check dpad states vs    state, if any of them are true, add/subtract one
		if (copilot->GetAxis(SmartJoypad::DPAD_X) != 0)
			leftright += (int) copilot->GetAxisDelta(SmartJoypad::DPAD_X);
		
		if (copilot->GetAxis(SmartJoypad::DPAD_Y) != 0)
			updown -= (int) copilot->GetAxisDelta(SmartJoypad::DPAD_Y);
		
		/********************* State Updates (High Priority) *********************/
		
		shooter->Update();
		
		/******************** Pilot Input ********************/
		
		// Avoid any drive timeouts.
		drive->TankDrive(0.0, 0.0);
		
		if (tankDrive)
			drive->TankDrive(
				pilot->GetAxis(SmartJoypad::STICK_LY), 
				pilot->GetAxis(SmartJoypad::STICK_RY)
			);
		else
			drive->ArcadeDrive(	
				pilot->GetAxis(SmartJoypad::STICK_LY),
				pilot->GetAxis(SmartJoypad::STICK_RX),
				USE_SQUARED_INPUTS
			);
		
		if (pilot->GetButton(SmartJoypad::BUMPER_L2))
		{
			
			if (bridgeArmSwitchLow->Get())
				bridgeArm->Set(0.2);
			else
				bridgeArm->Set(0.0);
		}
		else if (pilot->GetButton(SmartJoypad::BUMPER_L1))
		{
			if (bridgeArmSwitchHigh->Get())			
								bridgeArm->Set(-1.0);
			else
			{
				bridgeArm->Set(0.0);
			}
		}
		else
			bridgeArm->Set(0.0);
		
		
		if (pilot->GetButton(SmartJoypad::BUMPER_R1))
			gearShifter->Set(DoubleSolenoid::kReverse);
		else if (pilot->GetButton(SmartJoypad::BUMPER_R2))
			gearShifter->Set(DoubleSolenoid::kForward);
		
		/******************** Copilot Input ********************/
		
		if (copilot->GetButton(SmartJoypad::BUMPER_L1))
		{
			turret->SetBallConveyor(1.0);
			turret->SetBallfeed(1.0f);
			turret->SetFlippyBar(FLIPPY_BAR_OPEN);
		}
		else if (copilot->GetButton(SmartJoypad::BUMPER_L2))
		{
			turret->SetBallConveyor(-1.0);
			turret->SetBallfeed(-1.0f);
			turret->SetFlippyBar(FLIPPY_BAR_OPEN);
		}
<<<<<<< .mine
		else if (copilot->GetButton(SmartJoypad::BUMPER_R1))
		{
			if((shooter->UpToSpeed()) && (firingState == 4))
			{
				turret->SetBallConveyor(1.0);
				turret->SetBallfeed(1.0f);
				turret->SetFlippyBar(FLIPPY_BAR_CLOSE);
			}
		}
		else
=======
		else if (!copilot->GetButton(SmartJoypad::BUMPER_R1))
>>>>>>> .r122
		{
			turret->SetBallConveyor(0.0);
			turret->SetBallfeed(0.0f);
		}
<<<<<<< .mine

=======
		
		
		/*if (copilot->GetButton(SmartJoypad::FACE_N))
			shooter->SetRawSpeed(1.0);*/
		
>>>>>>> .r122
		//Autofire Controls
<<<<<<< .mine
		if (copilot->GetButton(SmartJoypad::BUMPER_R2) && camera->FindTarget(distance))
=======
		if (copilot->GetButton(SmartJoypad::BUMPER_R2))
>>>>>>> .r122
		{
			if (firingState == 0) firingState = 1;
		}
		else if (firingState != 0)
		{

			firingState = 4;
		}

		if (copilot->GetButton(SmartJoypad::BUMPER_R1)){
			this->MoveConveyor();
		}
		else
		{
			if (!copilot->GetButton(SmartJoypad::BUMPER_R2)){
				turret->SetFlippyBar(FLIPPY_BAR_OPEN);
				turret->SetBallConveyor(0.0f);
				turret->SetBallConveyor(0.0f);
			}
		}
		
		
		shooterSpeed 	= shooter->ShootTo(distance + DISTANCE_OFFSET);
		//shooterSpeed 	=  atof(dashboard->GetString("RPS:").c_str()),
		
		/******************* State Machines ********************/
		
		// Handles shooter state control. firingState is changed in AutoFire.
<<<<<<< .mine
		spinFlywheel(firingState, (int) shooterSpeed);
=======
		//AutoFire(firingState, (int) shooterSpeed);
		SpinFlywheel();
>>>>>>> .r122
		
		if (copilot->GetButton(SmartJoypad::FACE_S))
		{
			turret->SetBallConveyor(1.0f);
			turret->SetBallfeed(1.0f);
			turret->SetFlippyBar(FLIPPY_BAR_CLOSE);
		}
		
		/******************* Dashboard Updates ********************/
		
		//turret->Print();
		shooter->Print();
		
		printf("Distance: %0.2f\n", distance + DISTANCE_OFFSET);
		
		pilot->Remember();
		copilot->Remember();
	}
};

START_ROBOT_CLASS(Team830Robot);
